#include "MPU6050.h"

I2C_TypeDef *MPU6050_I2Cx;

bool MPU6050_exist = false;
MPU6050Struct MPU6050;
uint8_t* MPU6050_BUF = NULL;

/**
 * @brief
 *
 * @param I2Cx I2C口选择: I2C1 或 I2C2
 */
void MPU6050_init(I2C_TypeDef *I2Cx)
{
    MPU6050_I2Cx = I2Cx;
    // MPU6050_turn(on);
    I2C_DeInit(I2Cx);
    I2C_init(I2Cx, MPU6050_SLAVE_ADDRESS_0);
    MPU6050_BUF = I2C_pointer(I2Cx)->BUF;
    Delay_ms(50);
    MPU6050_regInit();
    Delay_ms(50);
    MPU6050_compensate();
    MPU6050_exist = true;
}

void MPU6050_regInit()
{
    MPU6050_writeReg(PWR_MGMT_1, 0x80); // 复位
    Delay_ms(50);
    MPU6050_writeReg(PWR_MGMT_1, 0x00);             // 唤醒
    MPU6050_writeReg(PWR_MGMT_2, 0x00);             // 关闭待机
    MPU6050_writeReg(INT_ENABLE, 0x00);             // 关闭中断
    MPU6050_setFilterBandwidth(MPU6050_BAND_94_HZ); // 设置低通滤波器
    MPU6050_setGyroRange(MPU6050_RANGE_1000_DEG);   // 陀螺仪满量程
    MPU6050_setAccelRange(MPU6050_RANGE_16_G);      // 加速度满量程
    MPU6050_writeReg(INT_PIN_CFG, 0x02);
    MPU6050_writeReg(USER_CTRL, 0x00);
}

void MPU6050_waitBusy()
{
    I2C_waitBusy(MPU6050_I2Cx);
}

/**
 * @brief 向寄存器中写入数据
 *
 * @param reg 寄存器地址
 * @param Data 写入的数据
 */
void MPU6050_writeReg(uint8_t reg, uint8_t Data)
{
    I2C_writeReg(MPU6050_I2Cx, reg, Data);
}

void MPU6050_readReg(uint8_t reg, uint8_t size)
{
    I2C_readReg(MPU6050_I2Cx, reg, size);
}

void MPU6050_getBUFData(void)
{
    double accel_range = (int8_t)MPU6050.ACCEL_RANGE;
    double gyro_range = (int16_t)MPU6050.GYRO_RANGE;
    double temp;
    temp = (int16_t)(((uint16_t)(MPU6050_BUF[0]) << 8) | (MPU6050_BUF[1]));
    MPU6050.accel_x = temp * accel_range / 32767;
    temp = (int16_t)(((uint16_t)(MPU6050_BUF[2]) << 8) | (MPU6050_BUF[3]));
    MPU6050.accel_y = temp * accel_range / 32767;
    temp = (int16_t)(((uint16_t)(MPU6050_BUF[4]) << 8) | (MPU6050_BUF[5]));
    MPU6050.accel_z = temp * accel_range / 32767;
    temp = (int16_t)(((uint16_t)(MPU6050_BUF[8]) << 8) | (MPU6050_BUF[9]));
    MPU6050.gyro_x = temp * gyro_range / 32767;
    temp = (int16_t)(((uint16_t)(MPU6050_BUF[10]) << 8) | (MPU6050_BUF[11]));
    MPU6050.gyro_y = temp * gyro_range / 32767;
    temp = (int16_t)(((uint16_t)(MPU6050_BUF[12]) << 8) | (MPU6050_BUF[13]));
    MPU6050.gyro_z = temp * gyro_range / 32767;
    MPU6050.yaw += (MPU6050.gyro_z - MPU6050.GYROZ_0) * (double)(MPU6050_PERIOD) / 1000 * MPU6050_YAW_COMP;
    MPU6050.velo_x += (MPU6050.accel_x - MPU6050.ACCELX_0) * (double)(MPU6050_PERIOD) / 1000;
    MPU6050.velo_y += (MPU6050.accel_y - MPU6050.ACCELY_0) * (double)(MPU6050_PERIOD) / 1000;
    MPU6050.velo_z += (MPU6050.accel_z - MPU6050.ACCELZ_0) * (double)(MPU6050_PERIOD) / 1000;
    MPU6050.x += (MPU6050.velo_x) * (double)(MPU6050_PERIOD) / 1;
    MPU6050.y += (MPU6050.velo_y) * (double)(MPU6050_PERIOD) / 1;
    MPU6050.z += (MPU6050.velo_z) * (double)(MPU6050_PERIOD) / 1;
}

void MPU6050_compensate()
{
    uint8_t i;
    double accel_range = (int8_t)MPU6050.ACCEL_RANGE;
    double gyro_range = (int16_t)MPU6050.GYRO_RANGE;
    double temp;
    MPU6050.ACCELX_0 = 0;
    MPU6050.ACCELY_0 = 0;
    MPU6050.ACCELZ_0 = 0;
    MPU6050.GYROX_0 = 0;
    MPU6050.GYROY_0 = 0;
    MPU6050.GYROZ_0 = 0;
    for (i = 0; i < 100; i++)
    {
        MPU6050_readReg(MPU6050_DATA, 14);
        MPU6050_waitBusy();
        temp = (int16_t)(((uint16_t)(MPU6050_BUF[0]) << 8) | (MPU6050_BUF[1]));
        MPU6050.ACCELX_0 += (temp * accel_range / 32767 / 100);
        temp = (int16_t)(((uint16_t)(MPU6050_BUF[2]) << 8) | (MPU6050_BUF[3]));
        MPU6050.ACCELY_0 += (temp * accel_range / 32767 / 100);
        temp = (int16_t)((((uint16_t)MPU6050_BUF[4]) << 8) | (MPU6050_BUF[5]));
        MPU6050.ACCELZ_0 += (temp * accel_range / 32767 / 100);
        temp = (int16_t)((((uint16_t)MPU6050_BUF[8]) << 8) | (MPU6050_BUF[9]));
        MPU6050.GYROX_0 += (temp * gyro_range / 32767 / 100);
        temp = (int16_t)((((uint16_t)MPU6050_BUF[10]) << 8) | (MPU6050_BUF[11]));
        MPU6050.GYROY_0 += (temp * gyro_range / 32767 / 100);
        temp = (int16_t)(((uint16_t)(MPU6050_BUF[12]) << 8) | (MPU6050_BUF[13]));
        MPU6050.GYROZ_0 += (temp * gyro_range / 32767 / 100);
    }
}

void MPU6050_setGyroRange(uint8_t MPU6050_RANGE)
{
    uint8_t temp = 0;
    MPU6050_readReg(GYRO_CONFIG, 1);
    switch (MPU6050_RANGE)
    {
    case 0:
        MPU6050.GYRO_RANGE = 250;
        break;
    case 1:
        MPU6050.GYRO_RANGE = 500;
        break;
    case 2:
        MPU6050.GYRO_RANGE = 1000;
        break;
    case 3:
        MPU6050.GYRO_RANGE = 2000;
        break;
    }
    MPU6050_waitBusy();
    temp = MPU6050_BUF[0];
    temp |= (MPU6050_RANGE << 3);
    MPU6050_writeReg(GYRO_CONFIG, temp);
}

void MPU6050_setAccelRange(uint8_t MPU6050_RANGE)
{
    uint8_t temp = 0;
    MPU6050_readReg(ACCEL_CONFIG, 1);
    switch (MPU6050_RANGE)
    {
    case 0:
        MPU6050.ACCEL_RANGE = 2;
        break;
    case 1:
        MPU6050.ACCEL_RANGE = 4;
        break;
    case 2:
        MPU6050.ACCEL_RANGE = 8;
        break;
    case 3:
        MPU6050.ACCEL_RANGE = 16;
        break;
    }
    MPU6050_waitBusy();
    temp = MPU6050_BUF[0];
    temp |= (MPU6050_RANGE << 3);
    MPU6050_writeReg(ACCEL_CONFIG, temp);
}

void MPU6050_setFilterBandwidth(uint8_t MPU6050_BAND)
{
    uint8_t temp = 0;
    MPU6050_readReg(CONFIG, 1);
    MPU6050_waitBusy();
    temp = MPU6050_BUF[0];
    temp |= MPU6050_BAND;
    MPU6050_writeReg(CONFIG, temp);
}
